import math
import sys

import numpy as np
import pandas as pd

import Common.MatrixProcess as mp
from Common.ControlEnum import forcemode
from scipy.spatial.transform import Rotation as R

class AddmittanceControl():
    def __init__(self, K6, B6, M6, FT6, dt, trans=True, teachmode=forcemode.FreeMode,democsv=""):

        # 通用变量初始化
        self.trans = trans
        self.teachmode = teachmode
        print(f'导纳算法刚度矩阵k={K6},b={B6},m={M6},dt={dt},是否转换力坐标：{trans}')
        self.goalFt = FT6  # 工具坐标系下的f3
        self.times = 6170
        self.i = 0
        self.coffient=0.2
        self.time_stamp=0
        # region 力模式初始化
        # if (teachmode == forcemode.TeachMode):
        self.saveforce = []  # 示教力
        self.savepose=[]
        self.savetimestamp=[]
        if(teachmode == forcemode.ReplayMode):
            self.readforce = []  # 复现示教的力
            # df = pd.read_csv("result/demotraj/demo1.csv")
            df = pd.read_csv(democsv)

            self.readforce = np.array([np.fromstring(f.strip('[]'), sep=' ') for f in df['force']])
        else:
            None
        # endregion

        # region POS变量初始化
        self.k = K6  # pos
        self.b = B6
        self.m = M6

        self.delta_pose_t0 = np.zeros((6))
        self.delta_vel_t0 = np.zeros((6))
        self.delta_acc_t0 = np.zeros((6))

        self.delta_pose_t1 = np.zeros((6))
        self.delta_vel_t1 = np.zeros((6))
        self.delta_acc_t1 = np.zeros((6))
        self.goalpose_old = np.zeros((6))
        self.goalpose_v = np.zeros((6))

        self.goalpose_v_old = np.zeros((6))
        self.goalpose_a = np.zeros((6))

        self.dt_p = dt
        self.dt_o=dt*120
        # endregion

    def calpose(self,curpose0,goalpose0,curforce0):
        """
        Args:
            curpose:当前位置，从机器人读取上来的
            goalpose: 目标轨迹，世界坐标系下的
            curvel:
            curforce:

        Returns:
        """

        ###1
        # print(curforce)
        curforce=curforce0.copy()
        curpose=curpose0.copy()
        goalpose=goalpose0.copy()
        # curforce=np.array([0.1,0.1,0.1,0.1,0.1,0.1])
        Ft=self.FT_Trans(self.trans,curpose,curforce)
        #print(curforce)
        # Ft=np.array([0,0,0,5,5,0])
        force=Ft[0:3]
        torque=Ft[3:6]

        ###region 2计算目标速度，加速度
        if(self.i ==0):
            #计算旋转方向的目标速度和加速度
            delta=self.__cal_eular_delta_vec(goalpose[3:6],curpose[3:6])#将旋转向量换算成角度
            self.goalpose_v[3:6] = delta/self.dt_o  #给速度初始化,只执行一次
            self.goalpose_a[3:6] = 2*(goalpose[3:6]-curpose[3:6])/self.dt_o/self.dt_o# 加速度初始化 用x=1/2*a*t^2计算
            # 计算平移方向的目标速度和加速度
            delta = goalpose[0:3] - curpose[0:3]
            self.goalpose_v[0:3] = (delta)/self.dt_p
            self.goalpose_a[0:3] = 2*(goalpose[0:3]-curpose[0:3])/self.dt_p/self.dt_p

        else:
            #计算旋转方向的目标速度和加速度
            delta=self.__cal_eular_delta_vec(goalpose[3:6],self.goalpose_old[3:6])#将旋转向量换算成角度
            self.goalpose_v[3:6]= delta/self.dt_o
            self.goalpose_a[3:6] = (self.goalpose_v[3:6] - self.goalpose_v_old[3:6]) / self.dt_o
            # 计算平移方向的目标速度和加速度
            delta = goalpose[0:3] - self.goalpose_old[0:3]
            self.goalpose_v[0:3] = delta / self.dt_p
            self.goalpose_a[0:3] = (self.goalpose_v[0:3] - self.goalpose_v_old[0:3]) / self.dt_p

        self.goalpose_old = goalpose.copy()#获取当前的目标姿态
        self.goalpose_v_old = self.goalpose_v.copy()#获取当前的目标速度
        #endregion

        ###3,计算力误差
        torque[np.abs(torque)<0.2]=0
        force[np.abs(force) < 2] = 0
        if (abs(force[1]) < 0.5 * abs(force[2])):
            force[1] = 0
        if (abs(force[0]) < 0.5 * abs(force[2])):
            force[0] = 0
        Fe=self.forceCal(self.teachmode,Ft,isuseActive=True,isPose=True)#计算力误差
        Te=self.forceCal(self.teachmode,Ft,isuseActive=False,isPose=False)#计算力矩误差
        FTe=np.concatenate((Fe, Te))#合力TF的误差
        # if (self.teachmode == forcemode.TeachMode):
        self.saveforce.append(Ft)#保存原始力
        self.savepose.append(np.array(curpose))
        self.savetimestamp.append(self.time_stamp)

        ###4，根据加速度计算速度
        Ve=self.delta_vel_t0
        Xe=self.delta_pose_t0
        self.delta_acc_t1 = (1 / self.m) * (FTe - self.b * Ve)
        self.delta_vel_t1[0:3]= Ve[0:3] + self.delta_acc_t1[0:3] * self.dt_p#仅dt不同
        self.delta_vel_t1[3:6]= Ve[3:6] + self.delta_acc_t1[3:6] * self.dt_o#仅dt不同
        # self.delta_vel_t1= Ve + self.delta_acc_t1 * self.dt

        ###4.1 限制速度
        sump = np.linalg.norm(self.delta_vel_t1[3:])
        if (sump > 0.7):
            self.delta_vel_t1[3:] = (0.7 / sump) * self.delta_vel_t1[3:]
        sump = np.linalg.norm(self.delta_vel_t1[:3])
        if (sump > 0.1):
            self.delta_vel_t1[:3] = (0.0006 / sump) * self.delta_vel_t1[:3]

        ###4.2 根据速度，计算位移，角度增量
        # self.delta_pose_t1= Xe + self.delta_vel_t1 * self.dt#x0+vt
        self.delta_pose_t1[:3]= Xe[:3] + self.delta_vel_t1[:3] * self.dt_p
        self.delta_pose_t1[3:]= Xe[3:] + self.delta_vel_t1[3:] * self.dt_o
        self.delta_pose_t0=self.delta_pose_t1
        self.delta_vel_t0=self.delta_vel_t1

        movepose=self.__calcmdpose(goalpose,self.delta_pose_t1[:3],self.delta_pose_t1[3:])#
        self.i = self.i + 1
        return movepose#这个是最终的坐标位置，基坐标系下的

    def get_save_value(self):
        logdata = {'BaseForce': self.saveforce.copy(),
                   'actualTcpPose': self.savepose.copy(),
                   'muti_sensor_timestamp': self.savetimestamp.copy()}
        return logdata

    def FT_Trans(self,istrans,pose,force):
        """
        是否需要将力转化至基座标系下
        Args:
            istrans:
            pose:
            force:

        Returns:

        """
        if(istrans==True):
            RBase_Tcp = mp.rotation_matrix_from_vectorr(pose[3:])#旋转向量转旋转矩阵
            F = RBase_Tcp.dot(force[:3])#转化力矩
            T = RBase_Tcp.dot(force[3:])#转化力矩
            force[:3]=F
            force[3:]=T
            return force
        else:
            return force

    def forceCal(self,teachmode,curforce0,isuseActive,isPose):
        """
        根据模式，选择是复现力，还是记录力
        计算转化后的力
        """
        comp=0
        curforce=curforce0.copy()
        if(isPose==True):
            F=curforce[0:3]#力
            gF=self.goalFt[0:3]
            if (teachmode == forcemode.ReplayMode):
                i=math.floor(self.i / 1) * 1# 根据时间间隔，可以取5，10，20等
                # print(i)
                rF=self.readforce[i][0:3]#读取的示教力

            b=self.b[0:3]
            m = self.m[0:3]
            dt=self.dt_p
            goalposev=self.goalpose_v[0:3]
            goalpose_a=self.goalpose_a[0:3]
        else:
            F = curforce[3:6]  # 力
            gF = self.goalFt[3:6]
            if (teachmode == forcemode.ReplayMode):
                rF = self.readforce[self.i][3:6]  # 读取的示教力
            b = self.b[3:6]
            m = self.m[3:6]
            dt = self.dt_o
            goalposev = self.goalpose_v[3:6]
            goalpose_a = self.goalpose_a[3:6]
        if(isuseActive==True):
            comp=-goalposev * b-(m - b*dt)*goalpose_a

        if(teachmode==forcemode.TeachMode):
            #Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b-(self.m - self.b*self.dt)*self.goalpose_a[:3]
            Fe=F-gF+comp
            # np.savetxt("demoforce.txt", Fe)
        if(teachmode==forcemode.FreeMode):
            # Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b-(self.m - self.b*self.dt)*self.goalpose_a[:3]
            Fe=F-gF+comp
        if(teachmode==forcemode.ReplayMode):
            # curforce[:2] = 0.1*curforce[:2] + self.readforce[self.i][:2]
            # Fe = curforce[:3] - self.goalFt - self.goalpose_v[:3] * self.b - (self.m - self.b * self.dt) * self.goalpose_a[:3]
            crf=self.coffient*F+rF #以读取的力为主
            if(isPose==True):
                crf[2]=F[2]
            Fe=crf-gF+comp
        return Fe

    def __calcmdpose(self,goalpose,deltaP,deltaR):
        """

        Args:
            goalpose:目标位姿
            deltapose: 旋转方向的调整量

        Returns:调整后下发给机器人的位姿

        """
        movepose = np.zeros(6)
        movepose[0:3]=goalpose[0:3]  # 最终是加在goalpose上面了
        movepose[0]+=deltaP[0]#加上位置方向的增量
        movepose[1]+=deltaP[1]
        movepose[2]+=deltaP[2]
        #待定义，delatpose的形式是什么？是旋转矢量吗，还是角度增量。
        #答：是在XYZ三个方向上角度的增量，力直接引起的是绕XYZ轴三个方向的旋转
        pose_vec=self.__cal(goalpose,deltaR[0],deltaR[1],deltaR[2])#计算旋转矢量的增量
        # print("delta_pose_t1=",self.delta_pose_t1)
        movepose[3:] = pose_vec
        return movepose

    def __cal(self, curpose,x, y, z):
        """
        传入xyz方向的增量，以及当前的姿态，计算目标姿态
        Args:
            curpose:当前的位姿
            x:姿态X方向的增量
            y:姿态Y方向的增量
            z:姿态Z方向的增量
        Returns:

        """
        cur_pose = curpose
        cur_roate = cur_pose[3:]
        #print(f'当前旋转矢量{cur_roate}')

        # 将旋转角度转换为旋转对象（单位为弧度，需用np.radians转换）
        rotation_x = R.from_euler('x', np.radians(x), degrees=False)
        rotation_y = R.from_euler('y', np.radians(y), degrees=False)
        rotation_z = R.from_euler('z', np.radians(z), degrees=False)
        target_rotation = rotation_z * rotation_y * rotation_x  # 组合旋转：Z -> Y -> X
        #cal_eular(target_rotation)  # 验证一下结果
        # target_rotation = rotation_x * rotation_y * rotation_z
        current_rotation = R.from_rotvec(cur_roate)  # 计算当前的旋转矩阵
        # new_rotation = current_rotation * target_rotation#右乘
        new_rotation = target_rotation * current_rotation  # 左乘得到新的坐标
        new_rotation_vector = new_rotation.as_rotvec()  # 新姿态转化为旋转矢量，并返回

        ###打印验证可不用
        # cal_eular_delta(new_rotation, current_rotation.inv())
        # cur_euler = current_rotation.as_euler('xyz', degrees=True)
        # euler_angles = new_rotation.as_euler('xyz', degrees=True)
        # print(f'cureuler:{cur_euler},euler:{euler_angles}')
        ###
        return new_rotation_vector

    def __cal_eular_delta_vec(self,V1, V2):
        """
        delta = V1*V2.inv()#左乘
        Args:
            V1: 目标旋转向量
            V2: 当前旋转向量

        Returns:

        """
        R1 = R.from_rotvec(V1)
        R2 = R.from_rotvec(V2)
        eular = self.__cal_eular_delta(R1, R2.inv())
        #print(eular)
        return eular

    def __cal_eular_delta(self,R1, R2):
        R0 = R1 * R2
        R0 = R0.as_euler('xyz', degrees=True)
        R0 = np.round(R0, 2)
        #print(f'欧拉角差值{R0}')
        return R0

    def add_muti_sensor_time(self):
        """
        多传感器记录时的时间戳，每记录一次增加1
        Returns:

        """
        self.time_stamp+=1